#!/usr/bin/env python
# -*- coding: utf-8 -*-
#
# C++ version Copyright (c) 2006-2007 Erin Catto http://www.box2d.org
# Python version by Ken Lauer / sirkne at gmail dot com
#
# This software is provided 'as-is', without any express or implied
# warranty.  In no event will the authors be held liable for any damages
# arising from the use of this software.
# Permission is granted to anyone to use this software for any purpose,
# including commercial applications, and to alter it and redistribute it
# freely, subject to the following restrictions:
# 1. The origin of this software must not be misrepresented; you must not
# claim that you wrote the original software. If you use this software
# in a product, an acknowledgment in the product documentation would be
# appreciated but is not required.
# 2. Altered source versions must be plainly marked as such, and must not be
# misrepresented as being the original software.
# 3. This notice may not be removed or altered from any source distribution.
import pygame

from framework import (Framework, Keys, main)
from Box2D import (b2EdgeShape, b2FixtureDef, b2PolygonShape, b2_dynamicBody,
                   b2_kinematicBody, b2_staticBody)


class BodyTypes(Framework):
    name = ""
    # description = "Change body type keys: (d) dynamic, (s) static, (k) kinematic"
    speed = 3  # platform speed

    def __init__(self):
        super(BodyTypes, self).__init__()
        self.using_contacts = True
        # self.world.gravity = (0.0, 0.0)
        self.ground_body = self.world.CreateStaticBody(
            position=(0, 0),
            fixtures=b2FixtureDef(shape=b2PolygonShape(box=(50, 0)), density=2.0)
        )
        self.hanger = self.world.CreateStaticBody(
            position=(0, 40),
            fixtures=b2FixtureDef(shape=b2PolygonShape(box=(50, 0)), density=2.0)
        )
        # The attachment
        self.attachmentLeft = self.world.CreateDynamicBody(
            position=(-2, 0),
            fixtures=b2FixtureDef(shape=b2PolygonShape(box=(0.5, 2)), density=2.0)
        )
        self.attachmentRight = self.world.CreateDynamicBody(
            position=(2, 0),
            fixtures=b2FixtureDef(shape=b2PolygonShape(box=(0.5, 2)), density=2.0)
        )
        # he platform
        fixture = b2FixtureDef(
            shape=b2PolygonShape(box=(4, 0.5)),
            density=2,
            friction=0.6
        )
        self.platform = self.world.CreateDynamicBody(
            position=(0, 5),
            fixtures=fixture
        )
        # The joints joining the attachment/platform and ground/platform
        self.world.CreatePrismaticJoint(
            bodyA=self.attachmentLeft,
            bodyB=self.platform,
            anchor=(-2, 5),
            axis=(1, 0),
            maxMotorForce=1000,
            enableMotor=False,
            lowerTranslation=-2,
            upperTranslation=2,
            enableLimit=False
        )
        self.world.CreatePrismaticJoint(
            bodyA=self.attachmentRight,
            bodyB=self.platform,
            anchor=(2, 5),
            axis=(0, 1),
            maxMotorForce=1000,
            enableMotor=False,
            lowerTranslation=-2,
            upperTranslation=2,
            enableLimit=False
        )
        self.payload = self.world.CreateStaticBody(
            position=(0, 20),
            fixtures=b2FixtureDef(shape=b2PolygonShape(box=(50, 0.8)), density=1.0, friction=1)
        )
        self.leftAttachmentStatus = None  # 0: closed, 1: open, 2: grabbing something
        self.rightAttachmentStatus = None

    def Keyboard(self, key):
        if key == Keys.K_q:  # open the gripper
            if self.leftAttachmentStatus == 1:
                return
            fl = self.attachmentLeft.GetWorldVector(localVector=(-1000.0, 0))
            pl = self.attachmentLeft.GetWorldPoint(localPoint=(0, 0))
            self.attachmentLeft.ApplyForce(fl, pl, True)
            if self.rightAttachmentStatus == 1:
                return
            fr = self.attachmentRight.GetWorldVector(localVector=(1000, 0.0))
            pr = self.attachmentRight.GetWorldPoint(localPoint=(0.0, 0.0))
            self.attachmentRight.ApplyForce(fr, pr, True)
        elif key == Keys.K_e:  # close the gripper
            if self.leftAttachmentStatus == 0 or self.leftAttachmentStatus == 2:
                return
            fl = self.attachmentLeft.GetWorldVector(localVector=(1.0, 0))
            pl = self.attachmentLeft.GetWorldPoint(localPoint=(0, 0))
            self.attachmentLeft.ApplyForce(fl, pl, True)
            if self.rightAttachmentStatus == 0 or self.rightAttachmentStatus == 2:
                return
            fr = self.attachmentRight.GetWorldVector(localVector=(-1000, 0.0))
            pr = self.attachmentRight.GetWorldPoint(localPoint=(0.0, 0.0))
            self.attachmentRight.ApplyForce(fr, pr, True)
        elif key == Keys.K_s:
            if self.payload.transform.position.y <= 15:
                return
            self.payload.transform.position.y -= 0.1
        elif key == Keys.K_w:
            if self.payload.transform.position.y >= 25:
                return
            self.payload.transform.position.y += 0.1


    def Step(self, settings):
        super(BodyTypes, self).Step(settings)
        body_pairs = [(p['fixtureA'].body, p['fixtureB'].body) for p in self.points]
        for ip, (body1, body2) in enumerate(body_pairs):
            print(ip)
        posAttachment = self.attachmentLeft.transform.position
        posPlatform = self.platform.position
        posPlatformLeft = posPlatform.x - 4
        posPlatformLeftCener = posPlatform.x - 1
        posAttachmentLeft = posAttachment.x - 0.25
        attachmentLeftMovingDirection = None
        if self.attachmentLeft.linearVelocity[0] > 0:
            attachmentLeftMovingDirection = 'right'
        elif self.attachmentLeft.linearVelocity[0] < 0:
            attachmentLeftMovingDirection = 'left'
        if attachmentLeftMovingDirection == 'left' and posAttachmentLeft <= posPlatformLeft:
            self.attachmentLeft.linearVelocity = (0, 0)
            self.leftAttachmentStatus = 1  # is open
        if attachmentLeftMovingDirection == 'right' and posAttachmentLeft >= posPlatformLeftCener:
            self.attachmentLeft.linearVelocity = (0, 0)
            self.leftAttachmentStatus = 0  # is closed
        # for right attachment
        posAttachment = self.attachmentRight.transform.position
        posPlatform = self.platform.position
        posPlatformRight = posPlatform.x + 4
        posPlatformRightCener = posPlatform.x + 1
        posAttachmentRight = posAttachment.x + 0.25
        attachmentRightMovingDirection = None
        if self.attachmentRight.linearVelocity[0] > 0:
            attachmentRightMovingDirection = 'right'
        elif self.attachmentRight.linearVelocity[0] < 0:
            attachmentRightMovingDirection = 'left'
        if attachmentRightMovingDirection == 'left' and posAttachmentRight <= posPlatformRightCener:
            self.attachmentRight.linearVelocity = (0, 0)
            self.rightAttachmentStatus = 0  # is closed
        if attachmentRightMovingDirection == 'right' and posAttachmentRight >= posPlatformRight:
            self.attachmentRight.linearVelocity = (0, 0)
            self.rightAttachmentStatus = 1  # is open
        pygame.image.save(self.screen, 'outputImages/temp.png')


if __name__ == "__main__":
    main(BodyTypes)
